#include "MPU6050.h"

I2C_TypeDef *MPU6050_I2Cx = I2C2;

uint16_t busytime = 0;

bool MPU6050_exist = false;
bool MPU6050_send_flag = false;
bool MPU6050_receive_flag = false;
uint8_t MPU6050_receive_i = 0;
uint8_t MPU6050_receive_size = 0;
uint8_t MPU6050_reg = 0;
uint8_t MPU6050_Data = 0;
uint8_t MPU6050_BUF[MPU6050_BUF_SIZE];

MPU6050Struct MPU6050;

#define EV5 I2C_EVENT_MASTER_MODE_SELECT
#define EV6 I2C_EVENT_MASTER_TRANSMITTER_MODE_SELECTED
#define EV7 I2C_EVENT_MASTER_BYTE_RECEIVED
#define EV8 I2C_EVENT_MASTER_BYTE_TRANSMITTING
#define EV8_2 I2C_EVENT_MASTER_BYTE_TRANSMITTED

// typedef struct
// {
//   uint32_t I2C_ClockSpeed; 时钟频率 低于 400000
//   uint16_t I2C_Mode;
//   uint16_t I2C_DutyCycle;            时钟占空比
//   uint16_t I2C_OwnAddress1;          自身地址
//   uint16_t I2C_Ack;                  使能响应
//   uint16_t I2C_AcknowledgedAddress;  指定地址长度 7或10
// }I2C_InitTypeDef;

// typedef struct
// {
//   uint16_t GPIO_Pin;             /*!< Specifies the GPIO pins to be configured.
//                                       This parameter can be any value of @ref GPIO_pins_define */

//   GPIOSpeed_TypeDef GPIO_Speed;  /*!< Specifies the speed for the selected pins.
//                                       This parameter can be a value of @ref GPIOSpeed_TypeDef */

//   GPIOMode_TypeDef GPIO_Mode;    /*!< Specifies the operating mode for the selected pins.
//                                       This parameter can be a value of @ref GPIOMode_TypeDef */
// }GPIO_InitTypeDef;

void MPU6050_waitBusy()
{
    uint16_t Timeout = 1000;
    while (I2C_GetFlagStatus(MPU6050_I2Cx, I2C_FLAG_BUSY))
    {
        Timeout--;
        OLED_ShowNum(1, 10, ++busytime, 5);
        if (Timeout == 0)
        {
            busytime++;
            break;
        }
    }
}

void MPU6050_I2C_Configuration(I2C_TypeDef *I2Cx, uint32_t I2C_ClockSpeed, uint16_t I2C_DutyCycle)
{
    // 变量初始化
    I2C_InitTypeDef I2C_InitStruct;
    GPIO_InitTypeDef GPIO_InitStruct;
    NVIC_InitTypeDef NVIC_InitStruct;
    MPU6050_I2Cx = I2Cx;

    // 时钟配置
    // 配置 APB2EN 第 3 位为 IOPBEN 置1
    if (IS_BIT_0(RCC->APB2ENR, BIT(3)))
    {
        RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE);
    }
    if (IS_BIT_0(RCC->APB2ENR, BIT(0)))
    {
        RCC_APB2PeriphClockCmd(RCC_APB2Periph_AFIO, ENABLE);
    }
    // 配置 APB1EN 第 21 位为 I2C1EN 置1
    if (I2Cx == I2C1)
    {
        RCC_APB1PeriphClockCmd(RCC_APB1Periph_I2C1, ENABLE);
    }
    // 配置 APB1EN 第 22 位为 I2C2EN 置1
    if (I2Cx == I2C2)
    {
        RCC_APB1PeriphClockCmd(RCC_APB1Periph_I2C2, ENABLE);
    }

    // 配置 GPIO 为开漏复用输出，速度为50MHz
    if (I2Cx == I2C1)
    {
        GPIO_InitStruct.GPIO_Pin = GPIO_Pin_6 | GPIO_Pin_7;
    }
    if (I2Cx == I2C2)
    {
        GPIO_InitStruct.GPIO_Pin = GPIO_Pin_10 | GPIO_Pin_11;
    }
    GPIO_InitStruct.GPIO_Speed = GPIO_Speed_50MHz;
    GPIO_InitStruct.GPIO_Mode = GPIO_Mode_AF_OD;
    GPIO_Init(GPIOB, &GPIO_InitStruct);
    GPIO_SetBits(GPIOB, GPIO_InitStruct.GPIO_Pin);
    // 初始化I2C事件中断
    if (I2Cx == I2C1)
    {
        NVIC_InitStruct.NVIC_IRQChannel = I2C1_EV_IRQn;
    }
    if (I2Cx == I2C2)
    {
        NVIC_InitStruct.NVIC_IRQChannel = I2C2_EV_IRQn;
    }
    NVIC_InitStruct.NVIC_IRQChannelCmd = ENABLE;
    NVIC_InitStruct.NVIC_IRQChannelPreemptionPriority = 0;
    NVIC_InitStruct.NVIC_IRQChannelSubPriority = 0;
    NVIC_Init(&NVIC_InitStruct);
    I2C_ITConfig(I2Cx, I2C_IT_EVT, ENABLE);
    I2C_ITConfig(I2Cx, I2C_IT_BUF, ENABLE);

    // 根据初始化条件，配置 I2C
    I2C_InitStruct.I2C_ClockSpeed = I2C_ClockSpeed;
    I2C_InitStruct.I2C_Mode = I2C_Mode_I2C;
    I2C_InitStruct.I2C_DutyCycle = I2C_DutyCycle;
    I2C_InitStruct.I2C_OwnAddress1 = 0x00;
    I2C_InitStruct.I2C_Ack = I2C_Ack_Enable;
    I2C_InitStruct.I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit;
    I2C_Init(I2Cx, &I2C_InitStruct);

    I2C_Cmd(I2Cx, ENABLE);
}

void MPU6050_regInit()
{
    MPU6050_writeReg(PWR_MGMT_1, 0x80); // 复位
    Delay_ms(50);
    MPU6050_writeReg(PWR_MGMT_1, 0x00);             // 唤醒
    MPU6050_writeReg(PWR_MGMT_2, 0x00);             // 关闭待机
    MPU6050_writeReg(INT_ENABLE, 0x00);             // 关闭中断
    MPU6050_setFilterBandwidth(MPU6050_BAND_94_HZ); // 设置低通滤波器
    MPU6050_setGyroRange(MPU6050_RANGE_1000_DEG);   // 陀螺仪满量程
    MPU6050_setAccelRange(MPU6050_RANGE_16_G);      // 加速度满量程
    MPU6050_writeReg(INT_PIN_CFG, 0x02);
    MPU6050_writeReg(USER_CTRL, 0x00);
}

/**
 * @brief
 *
 * @param I2Cx I2C口选择: I2C1 或 I2C2
 * @param I2C_ClockSpeed: 时钟频率: 低于 400000 (Hz)
 * @param I2C_DutyCycle 时钟占空比: I2C_DutyCycle_16_9(16:9) 或 I2C_DutyCycle_2(2:1)
 */
void MPU6050_init(I2C_TypeDef *I2Cx, uint32_t I2C_ClockSpeed, uint16_t I2C_DutyCycle)
{
    I2C_DeInit(I2Cx);
    MPU6050_I2C_Configuration(I2Cx, I2C_ClockSpeed, I2C_DutyCycle);
    MPU6050_regInit();
    MPU6050_compensate();
    MPU6050_exist = true;
}

/**
 * @brief 向寄存器中写入数据
 *
 * @param reg 寄存器地址
 * @param Data 写入的数据
 */
void MPU6050_writeReg(uint8_t reg, uint8_t Data)
{
    MPU6050_waitBusy();
    MPU6050_Data = Data;
    MPU6050_send_flag = true;
    MPU6050_reg = reg;
    I2C_AcknowledgeConfig(MPU6050_I2Cx, ENABLE);
    I2C_GenerateSTART(MPU6050_I2Cx, ENABLE);
}

void MPU6050_readReg(uint8_t reg, uint8_t size)
{
    MPU6050_waitBusy();
    MPU6050_reg = reg;
    MPU6050_receive_flag = true;
    MPU6050_receive_size = size;
    MPU6050_receive_i = 0;
    I2C_AcknowledgeConfig(MPU6050_I2Cx, ENABLE);
    I2C_GenerateSTART(MPU6050_I2Cx, ENABLE);
}

void MPU6050_getBUFData(void)
{
    double accel_range = (int8_t)MPU6050.ACCEL_RANGE;
    double gyro_range = (int16_t)MPU6050.GYRO_RANGE;
    double temp;
    temp = (int16_t)(((uint16_t)(MPU6050_BUF[0]) << 8) | (MPU6050_BUF[1]));
    MPU6050.accel_x = temp * accel_range / 32767;
    temp = (int16_t)(((uint16_t)(MPU6050_BUF[2]) << 8) | (MPU6050_BUF[3]));
    MPU6050.accel_y = temp * accel_range / 32767;
    temp = (int16_t)(((uint16_t)(MPU6050_BUF[4]) << 8) | (MPU6050_BUF[5]));
    MPU6050.accel_z = temp * accel_range / 32767;
    temp = (int16_t)(((uint16_t)(MPU6050_BUF[8]) << 8) | (MPU6050_BUF[9]));
    MPU6050.gyro_x = temp * gyro_range / 32767;
    temp = (int16_t)(((uint16_t)(MPU6050_BUF[10]) << 8) | (MPU6050_BUF[11]));
    MPU6050.gyro_y = temp * gyro_range / 32767;
    temp = (int16_t)(((uint16_t)(MPU6050_BUF[12]) << 8) | (MPU6050_BUF[13]));
    MPU6050.gyro_z = temp * gyro_range / 32767;
    MPU6050.yaw += (MPU6050.gyro_z - MPU6050.GYROZ_0) * (double)(MPU6050_PERIOD) / 1000;
    MPU6050.velo_x += (MPU6050.accel_x - MPU6050.ACCELX_0) * (double)(MPU6050_PERIOD) / 1000;
    MPU6050.velo_y += (MPU6050.accel_y - MPU6050.ACCELY_0) * (double)(MPU6050_PERIOD) / 1000;
    MPU6050.velo_z += (MPU6050.accel_z - MPU6050.ACCELZ_0) * (double)(MPU6050_PERIOD) / 1000;
    MPU6050.x += (MPU6050.velo_x) * (double)(MPU6050_PERIOD) / 1;
    MPU6050.y += (MPU6050.velo_y) * (double)(MPU6050_PERIOD) / 1;
    MPU6050.z += (MPU6050.velo_z) * (double)(MPU6050_PERIOD) / 1;
}

void MPU6050_compensate()
{
    uint8_t i;
    double accel_range = (int8_t)MPU6050.ACCEL_RANGE;
    double gyro_range = (int16_t)MPU6050.GYRO_RANGE;
    double temp;
    MPU6050.ACCELX_0 = 0;
    MPU6050.ACCELY_0 = 0;
    MPU6050.ACCELZ_0 = 0;
    MPU6050.GYROX_0 = 0;
    MPU6050.GYROY_0 = 0;
    MPU6050.GYROZ_0 = 0;
    for (i = 0; i < 100; i++)
    {
        MPU6050_readReg(MPU6050_DATA, 14);
        MPU6050_waitBusy();
        temp = (int16_t)(((uint16_t)(MPU6050_BUF[0]) << 8) | (MPU6050_BUF[1]));
        MPU6050.ACCELX_0 += (temp * accel_range / 32767 / 100);
        temp = (int16_t)(((uint16_t)(MPU6050_BUF[2]) << 8) | (MPU6050_BUF[3]));
        MPU6050.ACCELY_0 += (temp * accel_range / 32767 / 100);
        temp = (int16_t)((((uint16_t)MPU6050_BUF[4]) << 8) | (MPU6050_BUF[5]));
        MPU6050.ACCELZ_0 += (temp * accel_range / 32767 / 100);
        temp = (int16_t)((((uint16_t)MPU6050_BUF[8]) << 8) | (MPU6050_BUF[9]));
        MPU6050.GYROX_0 += (temp * gyro_range / 32767 / 100);
        temp = (int16_t)((((uint16_t)MPU6050_BUF[10]) << 8) | (MPU6050_BUF[11]));
        MPU6050.GYROY_0 += (temp * gyro_range / 32767 / 100);
        temp = (int16_t)(((uint16_t)(MPU6050_BUF[12]) << 8) | (MPU6050_BUF[13]));
        MPU6050.GYROZ_0 += (temp * gyro_range / 32767 / 100);
    }
}

void MPU6050_setGyroRange(uint8_t MPU6050_RANGE)
{
    uint8_t temp = 0;
    MPU6050_readReg(GYRO_CONFIG, 1);
    switch (MPU6050_RANGE)
    {
    case 0:
        MPU6050.GYRO_RANGE = 250;
        break;
    case 1:
        MPU6050.GYRO_RANGE = 500;
        break;
    case 2:
        MPU6050.GYRO_RANGE = 1000;
        break;
    case 3:
        MPU6050.GYRO_RANGE = 2000;
        break;
    }
    MPU6050_waitBusy();
    temp = MPU6050_BUF[0];
    temp |= (MPU6050_RANGE << 3);
    MPU6050_writeReg(GYRO_CONFIG, temp);
}

void MPU6050_setAccelRange(uint8_t MPU6050_RANGE)
{
    uint8_t temp = 0;
    MPU6050_readReg(ACCEL_CONFIG, 1);
    switch (MPU6050_RANGE)
    {
    case 0:
        MPU6050.ACCEL_RANGE = 2;
        break;
    case 1:
        MPU6050.ACCEL_RANGE = 4;
        break;
    case 2:
        MPU6050.ACCEL_RANGE = 8;
        break;
    case 3:
        MPU6050.ACCEL_RANGE = 16;
        break;
    }
    MPU6050_waitBusy();
    temp = MPU6050_BUF[0];
    temp |= (MPU6050_RANGE << 3);
    MPU6050_writeReg(ACCEL_CONFIG, temp);
}

void MPU6050_setFilterBandwidth(uint8_t MPU6050_BAND)
{
    uint8_t temp = 0;
    MPU6050_readReg(CONFIG, 1);
    MPU6050_waitBusy();
    temp = MPU6050_BUF[0];
    temp |= MPU6050_BAND;
    MPU6050_writeReg(CONFIG, temp);
}

void I2C2_EV_IRQHandler(void)
{
    if (MPU6050_send_flag)
    {
        switch (I2C_GetLastEvent(MPU6050_I2Cx))
        {
            // EV5 SB 开始位已发送
        case I2C_EVENT_MASTER_MODE_SELECT:
            I2C_Send7bitAddress(MPU6050_I2Cx, MPU6050_SLAVE_ADDRESS_0, I2C_Direction_Transmitter);
            break;
            // EV6 ADDR 地址已发送
        case I2C_EVENT_MASTER_TRANSMITTER_MODE_SELECTED:
            I2C_SendData(MPU6050_I2Cx, MPU6050_reg);
            break;
        // EV8_2 TxE BTF 发送缓存区空 发送完成
        case I2C_EVENT_MASTER_BYTE_TRANSMITTED:
            I2C_SendData(MPU6050_I2Cx, MPU6050_Data);
            I2C_GenerateSTOP(MPU6050_I2Cx, ENABLE);
            MPU6050_send_flag = false;
            break;
        }
    }
    else if (MPU6050_receive_flag)
    {
        switch (I2C_GetLastEvent(MPU6050_I2Cx))
        {
            // EV5 SB 开始位已发送
        case I2C_EVENT_MASTER_MODE_SELECT:
            I2C_Send7bitAddress(MPU6050_I2Cx, MPU6050_SLAVE_ADDRESS_0, I2C_Direction_Transmitter);
            break;
            // EV6 ADDR 地址已发送
        case I2C_EVENT_MASTER_TRANSMITTER_MODE_SELECTED:
            I2C_SendData(MPU6050_I2Cx, MPU6050_reg);
            break;
            // EV8_2 TxE BTF 发送缓存区空 发送完成
        case I2C_EVENT_MASTER_BYTE_TRANSMITTED:
            I2C_GenerateSTART(MPU6050_I2Cx, ENABLE);
            MPU6050_receive_flag = false;
            break;
        }
    }
    else if (MPU6050_receive_size)
    {
        switch (I2C_GetLastEvent(MPU6050_I2Cx))
        {
            // EV5 SB 开始位已发送
        case I2C_EVENT_MASTER_MODE_SELECT:
            I2C_Send7bitAddress(MPU6050_I2Cx, MPU6050_SLAVE_ADDRESS_0, I2C_Direction_Receiver);
            break;
            // EV6 ADDR 地址已发送
        case I2C_EVENT_MASTER_RECEIVER_MODE_SELECTED:
            if (MPU6050_receive_size == 1)
            {
                I2C_AcknowledgeConfig(MPU6050_I2Cx, DISABLE);
                I2C_GenerateSTOP(MPU6050_I2Cx, ENABLE);
            }
            break;
            // EV7 RxNE 接收缓存区非空
        case I2C_EVENT_MASTER_BYTE_RECEIVED:
            MPU6050_BUF[MPU6050_receive_i++] = I2C_ReceiveData(MPU6050_I2Cx);
            if (MPU6050_receive_size == 1 + MPU6050_receive_i)
            {
                I2C_AcknowledgeConfig(MPU6050_I2Cx, DISABLE);
                I2C_GenerateSTOP(MPU6050_I2Cx, ENABLE);
            }
            break;
        }
    }
}
